#include <cmath>
#include <iostream>
#include "eigen/Eigen/Dense"

using namespace std;

// heading angle
//
//    ^ y（North）
//    |
//    |
//    o----> x(East)
//
// 定义：north -> 0, east -> 90, south -> 180, west -> 270
// 例如：
// x:  0.00, y:  1.00, HeadingAngle:  0.00
// x:  1.00, y:  0.00, HeadingAngle: 90.00
// x:  0.00, y: -1.00, HeadingAngle: 180.00

#define PI 3.141592653

double R2Degree(double rad) {
    return rad / PI * 180;
}

double CalculateAzimuth(const Eigen::Vector2d pt) {
    double azimuth = 0.0;

    azimuth = atan2(pt(0,0), pt(1,0));
    // azimuth = azimuth >= 0 ? azimuth : 2 * PI + azimuth;
    if (azimuth < 0) azimuth += 2 * PI;
    return R2Degree(azimuth);
}

int main() {
    std::cout << "azimuth Eigen::Vector2d(0, 1):" //
              << CalculateAzimuth(Eigen::Vector2d(0, 1)) << std::endl;
    std::cout << "azimuth Eigen::Vector2d(1, 0):" //
              << CalculateAzimuth(Eigen::Vector2d(1, 0)) << std::endl;
    std::cout << "azimuth Eigen::Vector2d(0, -1):" //
              << CalculateAzimuth(Eigen::Vector2d(0, -1)) << std::endl;
    std::cout << "azimuth Eigen::Vector2d(2, 2):"
              << CalculateAzimuth(Eigen::Vector2d(2, 2)) << std::endl;
    std::cout << "azimuth Eigen::Vector2d(2, 0):"
              << CalculateAzimuth(Eigen::Vector2d(2, 0)) << std::endl;
    std::cout << "azimuth Eigen::Vector2d(2, -2):"
              << CalculateAzimuth(Eigen::Vector2d(2, -2)) << std::endl;
    std::cout << "azimuth Eigen::Vector2d(0, -2):"
              << CalculateAzimuth(Eigen::Vector2d(0, -2)) << std::endl;
    std::cout << "azimuth Eigen::Vector2d(-2, -2):"
              << CalculateAzimuth(Eigen::Vector2d(-2, -2)) << std::endl;
    std::cout << "azimuth Eigen::Vector2d(-2, 0):"
              << CalculateAzimuth(Eigen::Vector2d(-2, 0)) << std::endl;
    std::cout << "azimuth Eigen::Vector2d(-2, 2):"
              << CalculateAzimuth(Eigen::Vector2d(-2, 2)) << std::endl;
    std::cout << "azimuth Eigen::Vector2d(-0.00001,  2):"
              << CalculateAzimuth(Eigen::Vector2d(-0.00001, 2)) << std::endl;
}